# ---------------ur rtde------------------
from .rtde_control import RTDEControlInterface as RTDEControl
from .rtde_receive import RTDEReceiveInterface as RTDEReceive
from .rtde_io import RTDEIOInterface as RTDEIO


class RobotControl(object):
    def __init__(self):
        self.rtde_c = RTDEControl("192.168.1.18")
        self.rtde_r = RTDEReceive("192.168.1.18")
        self.rtde_io_ = RTDEIO("192.168.1.18")  # IO
        self.end_z = 1
        self.velocity = 1.5
        self.acceleration = 1.5
        self.blend_1 = 0.05
        self.blend_2 = 0.02
        self.blend_3 = 0.0
        # wait point
        self.waitpoint = [0.27537, 0.24672, 0.42320, -1.48643, 2.7478, 0.00219]
        self.waitpoint_fast = [0.27537, 0.24672, 0.42320, -1.48643, 2.7478, 0.00219, self.velocity, self.acceleration,
                               self.blend_3]
        # self.p_waitpoint = [0.27537, 0.24672, 0.42320, -1.48643, 2.7478, 0.00219,
        #                     self.velocity,
        #                     self.acceleration,
        #                     self.blend_1]

        # put point
        # self.p_putpoint = [0.59476, 0.07366, 0.2500, -1.48643, 2.7478, 0.00219,
        #                    self.velocity,
        #                    self.acceleration,
        #                    self.blend_3]
        # setup cls
        # self.cls_1 = "spicy_strip"
        # self.cls_2 = "vegetarian_meat"

        # setup put position according to cls
        self.p_putpoint_1 = [0.59476, 0.28279, 0.00984, -1.48643, 2.7478, 0.00219,
                             self.velocity,
                             self.acceleration,
                             self.blend_3]
        # self.p_putpoint_2 = [0.59476, 0.06361, 0.00984, -1.48643, 2.7478, 0.00219,
        #                      self.velocity,
        #                      self.acceleration,
        #                      self.blend_3]
        # self.p_putpoint_3 = [0.59476, -0.15874, 0.00984, -1.48643, 2.7478, 0.00219,
        #                      self.velocity,
        #                      self.acceleration,
        #                      self.blend_3]

        # init to waitpoint
        self.rtde_c.moveL(self.waitpoint, self.velocity, self.acceleration, False)

        # init to open
        self.rtde_io_.setConfigurableDigitalOut(0, False)
        self.rtde_io_.setConfigurableDigitalOut(1, True)

    # def start(self, grasp_message, label):
    #     p_targetpoint = [grasp_message[0],
    #                      grasp_message[1],
    #                      grasp_message[2],
    #                      grasp_message[3],
    #                      grasp_message[4],
    #                      grasp_message[5],
    #                      self.velocity,
    #                      self.acceleration,
    #                      self.blend_3]
    #
    #     p_targetReadypoint = [grasp_message[0],
    #                           grasp_message[1],
    #                           grasp_message[2] + 0.18,
    #                           grasp_message[3],
    #                           grasp_message[4],
    #                           grasp_message[5],
    #                           self.velocity,
    #                           self.acceleration,
    #                           self.blend_2]
    #     # print(label)
    #     # if label == self.cls_1:
    #     #     p_putpoint = self.p_putpoint_1
    #     # elif label == self.cls_2:
    #     #     p_putpoint = self.p_putpoint_2
    #     # else:
    #     #     p_putpoint = self.p_putpoint_3
    #
    #     # put_path = [p_targetReadypoint, p_targetpoint, p_targetReadypoint, self.p_waitpoint, p_putpoint]
    #
    #     # close
    #     self.rtde_io_.setConfigurableDigitalOut(0, True)
    #     self.rtde_io_.setConfigurableDigitalOut(1, False)
    #
    #     # self.rtde_c.moveL(put_path)
    #
    #     # open
    #     self.rtde_io_.setConfigurableDigitalOut(0, False)
    #     self.rtde_io_.setConfigurableDigitalOut(1, True)
    #
    #     # go home
    #     self.rtde_c.moveL([self.waitpoint_fast])
    #     print('------------------reach put_path--------------')

    def get_TCPlocation(self):
        target = self.rtde_r.getActualTCPPose()
        return target

    def move_arm_to(self, new_position):
        tar_position = [new_position[0],
                        new_position[1],
                        new_position[2],
                        new_position[3],
                        new_position[4],
                        new_position[5],
                        self.velocity,
                        self.acceleration,
                        self.blend_3]
        self.rtde_c.moveL([tar_position])

    def grasp(self, new_position):
        end_position = [new_position[0],
                        new_position[1],
                        self.end_z,
                        new_position[3],
                        new_position[4],
                        new_position[5],
                        self.velocity,
                        self.acceleration,
                        self.blend_3]
        self.rtde_c.moveL([end_position, self.p_putpoint_1, self.waitpoint_fast])

    def return_origin(self):
        # init to waitpoint
        self.rtde_c.moveL(self.waitpoint, self.velocity, self.acceleration, False)
